/*
 *  Copyright (C) 2022  贺龙宇
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include "../Log/log.h"
#include "../Config/CameraParam.h"
#include "../SerialPort/Protocol.h"
#include "../Armor/ArmorDetector.h"

#include <string>
#include <opencv2/opencv.hpp>
#include "../define.h"

const float orbit_height=1.3; //哨兵轨道高度
const float g=9.8; //重力加速度
const float rad=57.295779513082320876798154814105; //弧度


typedef struct {
    float X_last; //ÉÏÒ»Ê±¿ÌµÄ×îÓÅœá¹û
    float X_mid;  //µ±Ç°Ê±¿ÌµÄÔ€²âœá¹û
    float X_now;  //µ±Ç°Ê±¿ÌµÄ×îÓÅœá¹û
    float P_mid;  //µ±Ç°Ê±¿ÌÔ€²âœá¹ûµÄÐ­·œ²î
    float P_now;  //µ±Ç°Ê±¿Ì×îÓÅœá¹ûµÄÐ­·œ²î
    float P_last; //ÉÏÒ»Ê±¿Ì×îÓÅœá¹ûµÄÐ­·œ²î
    float kg;     //kalmanÔöÒæ
    float A;      //ÏµÍ³²ÎÊý
    float Q;
    float R;
    float H;
	void Init(const float &first_data){
		X_last = first_data;
    	P_last = 0;
    	Q = 60;
    	R = 400;
    	A = 1;
    	H = 1;
    	X_mid = X_last;
	}
	float Filter(const float &data){
		X_mid =A*X_last;                     //x(k|k-1) = AX(k-1|k-1)+BU(k)
    	P_mid = A*P_last+Q;               //p(k|k-1) = Ap(k-1|k-1)A'+Q
    	kg = P_mid/(P_mid+R);             //kg(k) = p(k|k-1)H'/(Hp(k|k-1)'+R)
    	X_now = X_mid+kg*(data-X_mid);     //x(k|k) = X(k|k-1)+kg(k)(Z(k)-HX(k|k-1))
    	P_now = (1-kg)*P_mid;                //p(k|k) = (I-kg(k)H)P(k|k-1)
    	P_last = P_now;                         //×ŽÌ¬žüÐÂ
    	X_last = X_now;
    	return X_now;
	}
}Kalman;

/*卡尔曼滤波器*/
class KalmanFilter
{
public:
    KalmanFilter():init_flag(false){};	
    void Init();					//初始化一些矩阵
    cv::Mat Predict(cv::Mat& X_,cv::Mat& U_);		//用Update()后再预测
    cv::Mat Update(cv::Mat& X_,cv::Mat& Z_,cv::Mat& U_);//滤波
private:
    bool init_flag;
    cv::Mat X;	//状态向量 6x1
    cv::Mat Z;	//测量值 3x1
    cv::Mat F;	//状态转移矩阵,用于X到X_predict的转换 6x6
    cv::Mat U;	//加速度 3x1
    cv::Mat P;	//协方差矩阵 6x6
    cv::Mat U_cov;//U的协方差矩阵 3x3
    cv::Mat Q;	//过程噪声 6x6
    cv::Mat B;	//状态转移矩阵 用于U到X_predict的转换 6x3
    cv::Mat R;	//测量协方差矩阵 3x3
    cv::Mat H;	//状态转移矩阵 用于P到R的转换
    cv::Mat K;	
    cv::Mat X_predict;
    cv::Mat P_predict;
};


class SolvePos
{
public:
    SolvePos();
    ~SolvePos();
    cv::Point3f GetTarget(const cv::Mat &src, const std::vector<cv::Rect>& box, std::vector<int> &id);/*外部调用接口*/
    
    void TargetLost(); /*用于提醒目标丢失，在下次获得目标时进行小陀螺初始化*/
private:

#ifdef CNN
	// friend class ArmorDetector;
	float CalcDistance(const cv::Mat &src,const cv::Rect &final_armor);
	ArmorDetector armorDetector;
#endif

    cv::Mat show_final_armor;
    
    cv::Rect selected_rect;
    cv::Point last_rect_center; /*记录上一时刻的装甲板中心，用于反陀螺识别*/
    int topping_id; /*用于记录当前正在进行小陀螺的兵种id*/
    bool topping_init;/*指明反小陀螺是否需要重新初始化，用于应对目标丢失情况*/
    int frame_count; /*记录当前是连续识别的第几帧*/
    double hold_gain; /*小陀螺保持系数*/
    int min_frame; /*小陀螺最少间隔帧*/
    int max_frame; /*小陀螺最大间隔帧*/
    int given_frame; /*小陀螺帧数*/
    Kalman kf; /*小陀螺的kalman算法*/
    KalmanFilter kalman;
    
    int selected_rect_id;/*最终选择的装甲板ID*/
    int weights[9]={0,6,2,4,4,4,2,4,2}; /*兵种权重，优先英雄，步兵哨兵众生平等,最后是工程和建筑物*/

    
    cv::Point3f SolveTarget(const cv::Mat &src, const cv::Rect &armor,const int id); /*混合滤波的坐标解算*/
    float CalcDistancePnP(const cv::Mat &src, const cv::Rect &armor, const int id,const float dist_box);

    void ArmorFilter(std::vector<int> &id);/*装甲板过滤，只在旧的神经网络代码中有用*/
    void SelectArmor_AUTO(const std::vector<cv::Rect>& box, const std::vector<int> &id, cv::Rect &selected_rect);/*在所有装甲板中选出最优装甲板*/
    void SelectArmor_MANUAL(const std::vector<cv::Rect>& box, const std::vector<int> &id, cv::Rect &selected_rect);/*选择离图像中心最近的目标*/
    void SelectOutpost(const std::vector<cv::Rect>& box, const std::vector<int> &id, cv::Rect &selected_rect); /*选择前哨站*/
    void DistanceFilter(cv::Point3f &target, const int &id);/*测距值平滑处理*/
    cv::Point3f SolveControlInfo(const cv::Rect& armor);/*位置解算*/
    cv::Point3f AmmoEstimation(const cv::Point3f &target);/*弹道下坠补偿（已无用）*/
    cv::Point3f Predict(const cv::Point3f& src_target);/*预测（已无用）*/
    cv::Rect OutpostPredict(const cv::Rect &selected_rect);
    cv::Point3f Target2GimbalAxis(const cv::Point3f &src_target);/*坐标变换（只对英雄有用）*/


    void ToppingAnalyzer(const std::vector<int> &id);/*用于分析是否需要开启小陀螺识别模式*/
    void TopInit(const cv::Rect &armor); /*反陀螺识别初始化*/
    cv::Rect AntiTop(const cv::Rect& armor);/*反小陀螺识别，返回应瞄准的装甲板区域*/ 


    // inline float GetDistance();   
    
};
